using BepuPhysics.Collidables;
using BepuUtilities;
using System;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.CollisionDetection.CollisionTasks
{
    // 单独的配对测试仪被设计为在狭窄阶段之外使用。它们需要可用于查询等,因此所有必要的数据都必须从外部收集。
    public struct SphereCapsuleTester : IPairTester<SphereWide, CapsuleWide, Convex1ContactManifoldWide>
    {
        public int BatchSize => 32;

        public void Test(ref SphereWide a, ref CapsuleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Test(ref SphereWide a, ref CapsuleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
        {
            // 球体-胶囊对的接触基于球体中心到胶囊内部线段的最近点。
            QuaternionWide.TransformUnitXY(orientationB, out var x, out var y);
            Vector3Wide.Dot(y, offsetB, out var t);
            t = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, -t));
            Vector3Wide.Scale(y, t, out var capsuleLocalClosestPointOnLineSegment);

            Vector3Wide.Add(offsetB, capsuleLocalClosestPointOnLineSegment, out var sphereToInternalSegment);
            Vector3Wide.Length(sphereToInternalSegment, out var internalDistance);
            // 请注意,按照惯例,法线从B指向A。这里,球体是A,胶囊是B,所以规格化需要一个否定。
            var inverseDistance = new Vector<float>(-1f) / internalDistance;
            Vector3Wide.Scale(sphereToInternalSegment, inverseDistance, out manifold.Normal);
            var normalIsValid = Vector.GreaterThan(internalDistance, Vector<float>.Zero);
            // 如果球体的中心位于内部线段上,则在由胶囊的上方向向量定义的平面上选择一个方向。
            // 我们早些时候计算了一个这样的候选人。请注意,我们通常可以选择一个完全任意的方向,但是
            // 如果用户正在尝试计算真实的局部水平方向,则通过额外的工作可以避免一些令人讨厌的拐角情况意外
            // 执行某些操作,如手动解决冲突或其他基于查询的逻辑。
            // 更便宜的选择是简单地使用y轴作为法线。众所周知,这是次优的,但如果我们不能保证最小穿透深度,那是完全可以的。
            // 我的猜测是,计算x将非常便宜,以至于无关紧要。
            Vector3Wide.ConditionalSelect(normalIsValid, manifold.Normal, x, out manifold.Normal);
            manifold.Depth = a.Radius + b.Radius - internalDistance;
            manifold.FeatureId = Vector<int>.Zero;

            // 相对于对象A(球体)的接触位置计算为每个图形上沿法线朝向相反图形的极值点的平均值,然后求平均值。
            // 对于胶囊球体,这可以从法线和深度计算。
            var negativeOffsetFromSphere = manifold.Depth * 0.5f - a.Radius;
            Vector3Wide.Scale(manifold.Normal, negativeOffsetFromSphere, out manifold.OffsetA);
            manifold.ContactExists = Vector.GreaterThan(manifold.Depth, -speculativeMargin);
        }

        public void Test(ref SphereWide a, ref CapsuleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }
    }
}
